#include "QEPDriver.h"   // Example specific Include file
#include "..\System\F2806x_Device.h"

void QEPDriver::Init(void)
{
	// set pins
	EALLOW;                       // Enable EALLOW
	GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1;  // GPIO20 is EQEP1A
	GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1;  // GPIO21 is EQEP1B
	GpioCtrlRegs.GPAMUX2.bit.GPIO22 = 1;  // GPIO22 is EQEP1S
	GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1;  // GPIO23 is EQEP1I 
 	EDIS;                         // Disable EALLOW
 	
	
	EQep1Regs.QUPRD=800000;         // Unit Timer for 100Hz at 80 MHz SYSCLKOUT
	
	EQep1Regs.QDECCTL.bit.QSRC=00;      // QEP quadrature count mode
	
	EQep1Regs.QEPCTL.bit.FREE_SOFT=2;
	EQep1Regs.QEPCTL.bit.PCRM=01;       // PCRM=01 mode - QPOSCNT Reset on Maximum Position
	EQep1Regs.QEPCTL.bit.SEI=3; 		// SEI=11 - QPOSCNT Init on falling edge of QEPS strobe (XXX: must go counterclockwise!!!)
	
	EQep1Regs.QEPCTL.bit.UTE=1;         // Unit Timeout Enable
	EQep1Regs.QEPCTL.bit.QCLM=1;        // Latch on unit time out
	EQep1Regs.QPOSCNT=101000; // reset value (non initialized!)
	EQep1Regs.QPOSINIT=100000; // init value (after strobe/init) 
	EQep1Regs.QPOSMAX=0xffffffff;
	EQep1Regs.QEPCTL.bit.QPEN=1;        // QEP enable
	
	EQep1Regs.QCAPCTL.bit.UPPS=1;       // 1/2 for unit position
	EQep1Regs.QCAPCTL.bit.CCPS=7;       // 1/128 for CAP clock (SYSCLKOUT/128)
	EQep1Regs.QCAPCTL.bit.CEN=1;        // QEP Capture Enable
	
 	
 	// reset values
 	m_StrobeLocked = false;
 	m_currentPosition = EQep1Regs.QPOSCNT;
 	m_lastLatchedPos = EQep1Regs.QPOSCNT;
 	m_Direction = 0;
 	m_SpeedFast = 0;
 	m_SpeedSlow = 0;
 	
 	// convert to rad/s
 	m_ToOmega = 1.0f/64 * 6.283185f; // 64 pulses per revolution
}

float QEPDriver::GetSpeed(void)
{
	float speed=0;
	
	if(fabs(m_SpeedFast) > 2000 ) // 2000 pulses per second
	{
		speed = m_SpeedFast;
	}
	else
	{
		speed = m_SpeedSlow;
	}
	
	speed = speed * m_ToOmega;
	
	return speed;
}

float QEPDriver::GetPosition(void)
{
	return (float)m_currentPosition-100000; // CNT is shifted for 100000 units (to avoid zero crossing)
}

void QEPDriver::Calc(void)
{
	// get current position
	m_currentPosition = EQep1Regs.QPOSCNT;
	m_Direction = EQep1Regs.QEPSTS.bit.QDF;
     
    // check strobe
    if( EQep1Regs.QFLG.bit.SEL == 1)
    {
    	m_StrobeLocked = true;
    }
     
	//**** High Speed Calcultation using QEP Position counter ****//
	// Unit Timer is configured for 100Hz in INIT function
    if(EQep1Regs.QFLG.bit.UTO==1)                    // If unit timeout (one 100Hz period)
    {
        /** Differentiator  **/
        Uint32 currentLatchedPos = EQep1Regs.QPOSLAT;                // Latched POSCNT value
        Int32 diff = currentLatchedPos - m_lastLatchedPos;
        m_SpeedFast = diff * 100; // pulses per second  
        
        m_lastLatchedPos = currentLatchedPos; // remember old state
        EQep1Regs.QCLR.bit.UTO=1;                   // Clear interrupt flag
    }
     
    //**** Low-speed computation using QEP capture counter ****//
    if(EQep1Regs.QEPSTS.bit.UPEVNT==1)                 // Unit position event
    {
    	Uint32 period = EQep1Regs.QCPRDLAT;
    	
    	float newSpeedSlow = 0;
    	if( EQep1Regs.QEPSTS.bit.COEF==0)
    	{
    		newSpeedSlow = 2*625000.0f/period; // 2 pulses per period
    	}
    	else
    	{
    		// overflow, set speed to ZERO
    		newSpeedSlow = 0;
    	}
    	
    	if( m_Direction==0 )
    	{
    		newSpeedSlow = -newSpeedSlow; // CCW, invert speed
    	}
      
      	m_SpeedSlow = newSpeedSlow;
      
        EQep1Regs.QEPSTS.all=0x88;                  // Clear Unit position event flag
                                                    // Clear overflow error flag
    }
     
    // check slow timer overflow (if motor is stopped!)
	if( EQep1Regs.QEPSTS.bit.COEF==1)
	{
		m_SpeedSlow = 0;
	}
}

bool QEPDriver::IsLocked(void)
{
	return m_StrobeLocked;
}
